using System.Diagnostics;

namespace Xfinity.Physics
{
    /// <summary>
    /// A class for solving ordinary differential equations.
    /// </summary>
    public static class OdeSolver
    {
        private static double[] dq1, dq2, dq3, dq4, q;

        /// <summary>
        /// Solve an ODE using the Runge-Kutta-4 method.
        /// </summary>
        /// <param name="ode">The ODE to solve.</param>
        /// <param name="deltaTime">The delta time.</param>
        public static void RungeKutta4(Ode ode, double deltaTime)
        {
            System.Diagnostics.Debug.Assert(ode != null);
            System.Diagnostics.Debug.Assert(deltaTime != 0);

            q = ode.DependantVariables;
            double halfTime = ode.Time + .5 * deltaTime;
            dq1 = ode.EvaluateRightHandSide(ode.Time, q, q, deltaTime, 0);
            dq2 = ode.EvaluateRightHandSide(halfTime, q, dq1, deltaTime, .5);
            dq3 = ode.EvaluateRightHandSide(halfTime, q, dq2, deltaTime, .5);
            dq4 = ode.EvaluateRightHandSide(ode.Time + deltaTime, q, dq3, deltaTime, 1);

            ode.Time += deltaTime;

            for (int i = 0; i < q.Length; i++)
            {
                q[i] = q[i] + ((dq1[i] + 2 * dq2[i] + 2 * dq3[i] + dq4[i]) / 6);
            }
        }
    }
}